#include <firmament.h>
#include "hal/actuator/actuator.h"

#include "qx_hw_pwm.h"
#include "pwm_config.h"


#define MAX_PWM_OUT_CHAN      10            // Main Out has 10 pwm channel

static rt_err_t pwm_config(actuator_dev_t dev, const struct actuator_configure* cfg);
static rt_err_t pwm_control(actuator_dev_t dev, int cmd, void* arg);
static rt_size_t pwm_read(actuator_dev_t dev, rt_uint16_t chan_sel, rt_uint16_t* chan_val, rt_size_t size);
static rt_size_t pwm_write(actuator_dev_t dev, rt_uint16_t chan_sel, const rt_uint16_t* chan_val, rt_size_t size);

const static struct actuator_ops __act_ops = {
    .act_config = pwm_config,
    .act_control = pwm_control,
    .act_read = pwm_read,
    .act_write = pwm_write
};

static struct actuator_device act_dev = {
    .chan_mask = 0x3FF,
    .range = { 1000, 2000 },
    .config = {
        .protocol = ACT_PROTOCOL_PWM,
        .chan_num = MAX_PWM_OUT_CHAN,
        .pwm_config = { .pwm_freq = 50 },
        .dshot_config = { 0 } },
    .ops = &__act_ops
};

/*------------------------------------------------------*/
static rt_err_t qx_hw_pwm_control(struct rt_device_pwm *device, int cmd, void *arg);

static struct rt_pwm_ops drv_ops =
{
    qx_hw_pwm_control,
};

static rt_err_t qx_hw_pwm_switch(struct rt_pwm_configuration *configuration, rt_bool_t enable)
{
    return RT_EOK;
}

static rt_err_t qx_hw_pwm_set(struct rt_pwm_configuration *configuration)
{
    return RT_EOK;
}

static rt_err_t qx_hw_pwm_set_period(struct rt_pwm_configuration *configuration)
{
    return RT_EOK;
}

static rt_err_t qx_hw_pwm_set_pulse(struct rt_pwm_configuration *configuration)
{
    return RT_EOK;
}

static rt_err_t qx_hw_pwm_get(struct rt_pwm_configuration *configuration)
{
    return RT_EOK;
}

/*****************************************************************************************************/

static rt_err_t qx_hw_pwm_control(struct rt_device_pwm *device, int cmd, void *arg)
{
    struct rt_pwm_configuration *configuration = (struct rt_pwm_configuration *)arg;

    switch (cmd)
    {
    case PWM_CMD_ENABLE:
        return qx_hw_pwm_switch(configuration, RT_TRUE);
    case PWM_CMD_DISABLE:
        return qx_hw_pwm_switch(configuration, RT_FALSE);
    case PWM_CMD_SET:
        return qx_hw_pwm_set(configuration);
    case PWM_CMD_SET_PERIOD:
        return qx_hw_pwm_set_period(configuration);
    case PWM_CMD_SET_PULSE:
        return qx_hw_pwm_set_pulse(configuration);
    case PWM_CMD_GET:
        return qx_hw_pwm_get(configuration);
    default:
        return -RT_EINVAL;
    }
}

static struct qx_pwm qx_pwm_obj[] =
{
#if defined(BOARD_CFG_PWM1)
        PMW1_CONFIG,
#endif

#if defined(BOARD_CFG_PWM2)
        PMW2_CONFIG,
#endif

#if defined(BOARD_CFG_PWM3)
        PMW3_CONFIG,
#endif

#if defined(BOARD_CFG_PWM4)
        PMW4_CONFIG,
#endif

#if defined(BOARD_CFG_PWM5)
        PMW5_CONFIG,
#endif

#if defined(BOARD_CFG_PWM6)
        PMW6_CONFIG,
#endif

#if defined(BOARD_CFG_PWM7)
        PMW7_CONFIG,
#endif

#if defined(BOARD_CFG_PWM8)
        PMW8_CONFIG,
#endif

#if defined(BOARD_CFG_PWM9)
        PMW9_CONFIG,
#endif

#if defined(BOARD_CFG_PWM10)
        PMW10_CONFIG,
#endif

#if defined(BOARD_CFG_PWM11)
        PMW11_CONFIG,
#endif

#if defined(BOARD_CFG_PWM12)
        PMW12_CONFIG,
#endif
};

const rt_uint8_t pwm_output_io_support_list[31] = {
        0,//GPIO_0
        1,//GPIO_1
        2,//GPIO_2
        3,//GPIO_3
        4,//GPIO_4
        5,//GPIO_5
        6,//GPIO_6
        7,//GPIO_7
        8,//GPIO_8
        9,//GPIO_9
        10,//GPIO_10
        11,//GPIO_11
        12,//GPIO_12
        13,//GPIO_13
        14,//GPIO_14
        15,//GPIO_15
        16,//GPIO_16
        17,//GPIO_17
        24,//GPIO_24
        28,//GPIO_28
        29,//GPIO_29
        30,//GPIO_30
        31,//GPIO_31
        32,//GPIO_32
        39,//GPIO_39
        40,//GPIO_40
        41,//GPIO_41
        56,//GPIO_56
        57,//GPIO_57
        58,//GPIO_58
        59,//GPIO_59
};

const GPIO_PinMux pwm_io_mux_value_list[31][1] = {
        {GPIO_0_EPWM1_A},
        {GPIO_1_EPWM1_B},
        {GPIO_2_EPWM2_A},
        {GPIO_3_EPWM2_B},
        {GPIO_4_EPWM3_A},
        {GPIO_5_EPWM3_B},
        {GPIO_6_EPWM4_A},
        {GPIO_7_EPWM4_B},
        {GPIO_8_EPWM5_A},
        {GPIO_9_EPWM5_B},
        {GPIO_10_EPWM6_A},
        {GPIO_11_EPWM6_B},
        {GPIO_12_EPWM7_A},
        {GPIO_13_EPWM7_B},
        {GPIO_14_EPWM8_A},
        {GPIO_15_EPWM8_B},
        {GPIO_16_EPWM5_A},
        {GPIO_17_EPWM5_B},
        {GPIO_24_EPWM8_A},
        {GPIO_28_EPWM7_A},
        {GPIO_29_EPWM7_B},
        {GPIO_30_EPWM9_A},
        {GPIO_31_EPWM9_B},
        {GPIO_32_EPWM8_B},
        {GPIO_39_EPWM10_A},
        {GPIO_40_EPWM10_B},
        {GPIO_41_EPWM6_A},
        {GPIO_56_EPWM11_A},
        {GPIO_57_EPWM11_B},
        {GPIO_58_EPWM12_A},
        {GPIO_59_EPWM12_B},
};

static rt_err_t qx_pwm_support_io_check(rt_uint8_t register_io)
{
    rt_err_t rst = RT_ERROR;

    for(rt_uint8_t i = 0; i < sizeof(pwm_output_io_support_list); i++){
        if(pwm_output_io_support_list[i] == register_io){
            rst = RT_EOK;
            break;
        }
    }

    return rst;
}

static void qx_pwm_io_config(rt_uint8_t register_io)
{
    GPIO_PinMux io_mux = 0xffUL;

    if(register_io < 18){
        io_mux = pwm_io_mux_value_list[register_io][0];
    }else if(register_io < 28){
        io_mux = pwm_io_mux_value_list[18][0];
    }else if(register_io < 39){
        io_mux = pwm_io_mux_value_list[18+(register_io - 28)][0];
    }else if(register_io < 56){
        io_mux = pwm_io_mux_value_list[24+(register_io - 39)][0];
    }else{
        io_mux = pwm_io_mux_value_list[27+(register_io - 56)][0];
    }

    if(io_mux != 0xffUL){
        GPIO_MuxConfig(register_io,io_mux,GPIO_PIN_TYPE_STD,GPIO_QUAL_SYNC);
    }
}

static rt_err_t qx_hw_pwm_init(struct qx_pwm* instance)
{
    /* no actually output channel */
    if(instance->output_ioA == 0xffUL && instance->output_ioB == 0xffUL){
        return RT_ERROR;
    }

    /* ioA check */
    if(instance->output_ioA != 0xffUL){
        if(qx_pwm_support_io_check(instance->output_ioA) != RT_EOK){
            return RT_ERROR;
        }

        qx_pwm_io_config(instance->output_ioA);
    }

    /* ioB check */
    if(instance->output_ioB != 0xffUL){
        if(qx_pwm_support_io_check(instance->output_ioB) != RT_EOK){
            return RT_ERROR;
        }

        qx_pwm_io_config(instance->output_ioB);
    }


    return RT_EOK;
}

static rt_err_t qx_pwm_init(void)
{
    /* first enable clock source digital clock gate */
    SysCtl_enablePeripheral(SYSCTL_PERIPH_CLK_TBCLKSYNC);

    for (rt_uint8_t i = 0; i < sizeof(qx_pwm_obj) / sizeof(qx_pwm_obj[0]); i++)
    {
        if(qx_hw_pwm_init(&qx_pwm_obj[i]) != RT_EOK){
            SysCtl_disablePeripheral(SYSCTL_PERIPH_CLK_TBCLKSYNC);
            return RT_ERROR;
        }else{
            if(rt_device_pwm_register(&qx_pwm_obj[i].pwm_device, qx_pwm_obj[i].name, &drv_ops, RT_NULL) != RT_EOK){
                SysCtl_disablePeripheral(SYSCTL_PERIPH_CLK_TBCLKSYNC);
                return RT_ERROR;
            }
        }
    }

    return RT_EOK;
}

/*****************************************************************************************************/

static rt_err_t pwm_config(actuator_dev_t dev, const struct actuator_configure* cfg)
{
    // DRV_DBG("aux out configured: pwm frequency:%d\n", cfg->pwm_config.pwm_freq);

    // if (__set_pwm_frequency(cfg->pwm_config.pwm_freq) != RT_EOK) {
    //     return RT_ERROR;
    // }
    /* update device configuration */
    dev->config = *cfg;

    return RT_EOK;
}

static rt_err_t pwm_control(actuator_dev_t dev, int cmd, void* arg)
{
    rt_err_t ret = RT_EOK;

    switch (cmd) {
    case ACT_CMD_CHANNEL_ENABLE:
        // /* set to lowest pwm before open */
        // for (uint8_t i = 0; i < MAX_PWM_OUT_CHAN; i++) {
        //     __write_pwm(i, VAL_TO_DC(act_dev.range[0]));
        // }

        // /* auto-reload preload enable */
        // timer_enable(TIMER0);
        // timer_enable(TIMER1);
        // timer_enable(TIMER3);
        break;
    case ACT_CMD_CHANNEL_DISABLE:
        // /* auto-reload preload disable */
        // timer_disable(TIMER0);
        // timer_disable(TIMER1);
        // timer_disable(TIMER3);
        break;
    case ACT_CMD_SET_PROTOCOL:
        /* TODO: Support dshot */
        ret = RT_EINVAL;
        break;
    default:
        ret = RT_EINVAL;
        break;
    }

    return ret;
}

static rt_size_t pwm_read(actuator_dev_t dev, rt_uint16_t chan_sel, rt_uint16_t* chan_val, rt_size_t size)
{
    rt_uint16_t* index = chan_val;
    float dc;

    for (uint8_t i = 0; i < MAX_PWM_OUT_CHAN; i++) {
        if (chan_sel & (1 << i)) {
            // __read_pwm(i, &dc);
            // *index = DC_TO_VAL(dc);
            // index++;
        }
    }

    return size;
}

static rt_size_t pwm_write(actuator_dev_t dev, rt_uint16_t chan_sel, const rt_uint16_t* chan_val, rt_size_t size)
{
    const rt_uint16_t* index = chan_val;
    rt_uint16_t val;
    float dc;

    for (uint8_t i = 0; i < MAX_PWM_OUT_CHAN; i++) {
        if (chan_sel & (1 << i)) {
            // val = *index;
            // /* calculate pwm duty cycle */
            // dc = VAL_TO_DC(val);
            // /* update pwm signal */
            // __write_pwm(i, dc);

            // index++;
        }
    }

    return size;
}

rt_err_t rt_hw_pwm_init(void)
{
    RT_CHECK(qx_pwm_init());

    /* register actuator hal device */
    return hal_actuator_register(&act_dev, "main_out", RT_DEVICE_FLAG_RDWR, NULL);
}
